    /*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package icontrole.core;

import java.util.List;

/**
 *
 * @author heliojr
 */
public class Controlador {

    private double Kp;
    private double Ki;
    private double Kd;
    private double erroAtual;
    private double erroPassado;
    private double saidaAtual;
    private double saidaPassada;
    private double sinalP;
    private double sinalI;
    private double sinalD;
    private double sinal_D;
    private boolean _condicional;
    //Tamanho do passo da derivada;
    private double h;
    //Valor desejado para o sistema de controle
    private double valorDesejado;
    //Tipo do controlador selecionado
    private int tipoControlador;
    //Constantes do espaco de estados
    private double const_k0;

    public double getConst_k0() {
        return const_k0;
    }

    public void setConst_k0(double const_k0) {
        this.const_k0 = const_k0;
    }

    public double getConst_k1() {
        return const_k1;
    }

    public void setConst_k1(double const_k1) {
        this.const_k1 = const_k1;
    }

    public double getConst_ki() {
        return const_ki;
    }

    public void setConst_ki(double const_ki) {
        this.const_ki = const_ki;
    }
    private double const_k1;
    private double const_ki;

    public Controlador() {
        erroAtual = erroPassado = 0;
        sinalP = sinalI = sinalD = 0;
        saidaAtual = saidaPassada = 0;
        //Passo da derivada
        h = 0.1;
        _condicional = false;

    }

    //Atualiza as variaveis que representam a saida no instante
    public void pegaSaida(double saidaDoSistema) {
        saidaPassada = saidaAtual;
        saidaAtual = saidaDoSistema;
    }

    //Antes de efetuar o calculo do erro, deve-se executar a funcao pegaSaida
    public void calculaErro() {
        erroPassado = erroAtual;
        erroAtual = getValorDesejado() - saidaAtual;
    }

    public void acaoProporcional() {
        sinalP = getKp() * getErroAtual();

    }

    public void acaoIntegral() {
        if (!_condicional) {
            sinalI = (getSinalI() + Ki * getErroAtual() * h);
        } else {
            if ((sinalI <= 3 && sinalI >= -3)
                    || (sinalI > 3 && erroAtual < 0)
                    || (sinalI < -3 && erroAtual > 0)) {

                sinalI = (getSinalI() + Ki * getErroAtual() * h);
            } else {
                sinalI = getSinalI();
                System.out.println("*************************Condicional*******************");
            }
        }
    }

    public void acaoDerivativa() {
        sinalD = (Kd * (erroAtual - erroPassado) / h);
    }

    public void acao_Derivativa() {
        sinal_D = Kd * (saidaAtual - saidaPassada) / h;
    }

    public double controleP() {
        calculaErro();
        acaoProporcional();
        return sinalP;
    }

    public double controlePI() {
        calculaErro();
        acaoProporcional();
        acaoIntegral();
        return sinalP + sinalI;
    }

    public double controlePD() {
        calculaErro();
        acaoProporcional();
        acaoDerivativa();
        return sinalP + sinalD;
    }

    public double controlePID() {
        calculaErro();
        acaoProporcional();
        acaoIntegral();
        acaoDerivativa();
        return sinalP + sinalI + sinalD;
    }

    public double controlePI_D() {
        calculaErro();
        acaoProporcional();
        acaoIntegral();
        acao_Derivativa();

        return sinalP + sinalI + sinal_D;
    }

    public double blocoDeControle() {
        switch (tipoControlador) {
            case 0:
                return controleP();
            case 1:
                return controlePI();
            case 2:
                return controlePD();
            case 3:
                return controlePID();
            case 4:
                return controlePI_D();
            default:
                return controlePI_D();

        }

    }

    /**
     * @return the Kp
     */
    public double getKp() {
        return Kp;
    }

    /**
     * @param Kp the Kp to set
     */
    public void setKp(double Kp) {
        this.Kp = Kp;
    }

    /**
     * @return the Ki
     */
    public double getKi() {
        return Ki;
    }

    /**
     * @param Ki the Ki to set
     */
    public void setKi(double Ki) {
        this.Ki = Ki;
    }

    /**
     * @return the Kd
     */
    public double getKd() {
        return Kd;
    }

    /**
     * @param Kd the Kd to set
     */
    public void setKd(double Kd) {
        this.Kd = Kd;
    }

    /**
     * @return the sinalP
     */
    public double getSinalP() {
        return sinalP;
    }

    /**
     * @param sinalP the sinalP to set
     */
    public void setSinalP(double sinalP) {
        this.sinalP = sinalP;
    }

    /**
     * @return the sinalI
     */
    public double getSinalI() {
        return sinalI;
    }

    /**
     * @param sinalI the sinalI to set
     */
    public void setSinalI(double sinalI) {
        this.sinalI = sinalI;
    }

    /**
     * @return the sinalD
     */
    public double getSinalD() {
        return sinalD;
    }

    /**
     * @param sinalD the sinalD to set
     */
    public void setSinalD(double sinalD) {
        this.sinalD = sinalD;
    }

    /**
     * @return the erroAtual
     */
    public double getErroAtual() {
        return erroAtual;
    }

    /**
     * @param erroAtual the erroAtual to set
     */
    public void setErroAtual(double erroAtual) {
        this.erroAtual = erroAtual;
    }

    /**
     * @return the erroPassado
     */
    public double getErroPassado() {
        return erroPassado;
    }

    /**
     * @param erroPassado the erroPassado to set
     */
    public void setErroPassado(double erroPassado) {
        this.erroPassado = erroPassado;
    }

    /**
     * @return the tipoControlador
     */
    public int getTipoControlador() {
        return tipoControlador;
    }

    /**
     * @param tipoControlador the tipoControlador to set
     */
    public void setTipoControlador(int tipoControlador) {
        this.tipoControlador = tipoControlador;
    }

    /**
     * @return the valorDesejado
     */
    public double getValorDesejado() {
        return valorDesejado;
    }

    /**
     * @param valorDesejado the valorDesejado to set
     */
    public void setValorDesejado(double valorDesejado) {
        this.valorDesejado = valorDesejado;
    }

    public boolean isCondicional() {
        return _condicional;
    }

    public void setCondicional(boolean _condicional) {
        this._condicional = _condicional;
    }

    public double controleEESR() {

        calculaErro();
        acao_Integral();
        return sinalI;

    }

    private void acao_Integral() {
        if (!_condicional) {
            sinalI = (getSinalI() + const_ki * getErroAtual() * h);
        } else {
            if ((sinalI <= 3 && sinalI >= -3)
                    || (sinalI > 3 && erroAtual < 0)
                    || (sinalI < -3 && erroAtual > 0)) {

                sinalI = (getSinalI() + const_ki * getErroAtual() * h);
            } else {
                sinalI = getSinalI();
                System.out.println("*************************Condicional*******************");
            }
        }
    }

    /**
     * Sistema graças a Deus
     * @param ut (colocar o retorno de controleEESR
     * @param ganho_L (l0 e l1)
     * @return 
     */
    public void FiveSystem(double ut) {
        matrix_E[0] = matrix_B[0] * ut + matrix_A[0] * matrix_X[0] + matrix_A[1] * matrix_X[1] + matrix_L[0] * (saidaAtual - escalar_Y);
        matrix_E[1] = matrix_B[1] * ut + matrix_A[2] * matrix_X[0] + matrix_A[3] * matrix_X[1] + matrix_L[1] * (saidaAtual - escalar_Y);

        matrix_I[0] = matrix_I[0] + matrix_E[0] * h;
        matrix_I[1] = matrix_I[1] + matrix_E[1] * h;
        matrix_X[0] = matrix_I[0];
        matrix_X[1] = matrix_I[1];

        escalar_Y = matrix_C[0] * matrix_X[0] + matrix_C[1] * matrix_X[1];


    }
    private double[] matrix_L = new double[2];
    private double[] matrix_A = new double[4];

    public void setMatrix_A(double[] matrix_A) {
        this.matrix_A = matrix_A;
    }

    public void setMatrix_B(double[] matrix_B) {
        this.matrix_B = matrix_B;
    }

    public void setMatrix_C(double[] matrix_C) {
        this.matrix_C = matrix_C;
    }

    public void setMatrix_I(double[] matrix_I) {
        this.matrix_I = matrix_I;
    }

    public void setMatrix_L(double[] matrix_L) {
        this.matrix_L = matrix_L;
    }
    private double[] matrix_B = new double[2];
    private double[] matrix_C = new double[2];
    private double[] matrix_E = new double[2];
    private double[] matrix_X = new double[2];
    private double[] matrix_I = new double[2];
    private double escalar_Y;

    public double getEscalar_Y() {
        return escalar_Y;
    }

    public double[] getMatrix_A() {
        return matrix_A;
    }

    public double[] getMatrix_B() {
        return matrix_B;
    }

    public double[] getMatrix_C() {
        return matrix_C;
    }

    public double[] getMatrix_E() {
        return matrix_E;
    }

    public double[] getMatrix_I() {
        return matrix_I;
    }

    public double[] getMatrix_L() {
        return matrix_L;
    }

    public double[] getMatrix_X() {
        return matrix_X;
    }

    private void setMatrix_L(double l0, double l1) {
        matrix_L[0] = l0;
        matrix_L[1] = l1;
    }

    private void setMatrix_A(double a00, double a01, double a10, double a11) {
        matrix_A[0] = a00;
        matrix_A[1] = a01;
        matrix_A[2] = a10;
        matrix_A[3] = a11;
    }

    private void setMatrix_B(double b0, double b1) {
        matrix_B[0] = b0;
        matrix_B[1] = b1;
    }

    private void setMatrix_C(double c0, double c1) {
        matrix_C[0] = c0;
        matrix_C[1] = c1;
    }
}
